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( SUMMARY 
' ^ 

This  papar  addresses  the  attitude  datermination,  acceleration  transformation,  and 
attitude/haading  output  computational  operations  performed  in  modern-day  strapdown  inertial 
navigation  systems.  Contemporary  algorithms  are  described  for  implementing  these  operations 
in  raal-time  computers.  The  attitude  determination  and  acceleration  transformation 
algorithm  discussions  are  baaad  on  the  two-speed  approach  in  which  high  frequency  coning  and 
sculling  effects  are  calculated  with  simplified  high  speed  algorithms,  with  results  fed  into 
lower  speed  higher  order  algorithms.  (-This  is  the  approach  that  is  typically  used  in  most 
modern-day  strapdown  systems.  Design ^equations  ara  includad  for  evaluating  the  performanca 
of  the  strapdown  computer  algorithms  as\a  function  of  con^>uter  execution  speed  and  sensor 
assembly  vibration  amplitude/ frequency/phaae  environment. 

Both  direction  cosine  and  quaternion  based  attitude  algorithms  are  described  and 
compared  in  light  of  modern-day  algorithm  accuracy  capabilities.  Orthogonality  and 
normalization  operations  are  addressed  for  potential  attitude  elgorithm  accuracy 
enhancement.  The  section  on  attitude  data  output  algorithms  includes  a discussion  on 
roll/yaw  Euler  angle  singularities  near  high/low  pitch  angle  conditions. 


1.  INTRODUCTION 

Ths  concept  of  strapdown  inertial  navigation  was  originatad  more  than  thirty  yaars  ago, 
largely  from  an  analytical  standpoint.  Tha  theoretical  analytical  expressions  for 
processing  etrapdawn  inertial  sensor  data  to  develop  attitude,  velocity,  and  position 
information  were  reasonably  well  understood  in  the  form  of  continuous  matrix  oparations  and 
differential  equations.  The  implementation  of  thesa  equations  in  a digitial  conputer, 
however,  was  invariably  keyed  to  severe  throughput  limitations  of  original  airborne  digitial 
computer  technology.  As  a result,  many  of  the  strapdown  computational  algorithms  originated 
during  these  early  periods  wsra  inherently  limited  in  accuracy,  particulary  under  high 
frequency  dynamic  motion.  A classical  test  for  elgorithm  accuracy  during  this  early  period 
wes  haw  well  the  algorithm  conputad  ettitude  undar  cyclic  coning  motion  as  the  coning 
frequency  approached  the  computer  updata  cycle  frequency. 

In  the  late  1960’s  and  aarly  1970’s,  several  analytical  efforts  eddrassed  tha  problem 
of  splitting  the  strapdown  computation  process  into  low  end  high  speed  sections  (7,  8,  10). 
Tha  low  spaed  section  contained  the  bulk  of  the  computational  equations,  end  wee  designed  to 
accurately  account  for  low  frequency  large  amplitude  dynamic  motion  effects  (a.g.,  vehicle 
maneuvering)-  Tha  high  speed  computetion  section  wes  designed  with  a small  sat  of  simple 
algorithms  that  would  accurataly  account  for  high  frequency  small  amplituda  dynamic  motion 
(e.g.,  vehicle  vibrations).  Splitting  the  computetional  process  in  this  mannar  allowed  the 
bulk  of  the  strapdown  algorithms  to  be  itarated  at  reesonabla  speeds  compatible  with 
computer  throughput  limitations.  The  high  speed  algorithms  wera  simple  anough  that  they 
could  be  mechanized  individually  with  special  purposa  alactronics,  or  es  a minor  high  speed 
loop  in  the  main  processor. 


Over  the  past  ten  yearB,  the  structura  of  most  etrapdawn  algorithms  hes  evolvad  into 
this  two  speed  structura.  Tha  techniquaB  have  been  refinad  today  so  that  fairly 
straight-forward  analytical  dasign  methods  can  be  usad  to  dafine  elgorithm  analytical  forme 
and  computational  rates  to  achieve  required  levels  of  performance  in  specified  dynamic 
anvironments. 

This  paper  daBcribes  the  algorithms  used  today  in  most  modern-day  strapdown  inertlel 
navigation  systams  to  calculate  ettitude  end  transform  acceleration  vector  measurements  from 
sensor  to  navigation  axes.  The  algorithms  for  integrating  the  transformed  accelerations 
into  velocity  and  position  data  are  not  addressad  because  it  is  believed  that  thase 
operations  are  generic  to  inertial  navigation  in  general.,  not  only  strapdown  inertial 
navigation. 

For  the  algorithms  discussad,  the  analytical  basis  is  prasantad  together  with  a 
discussion  on  general  design  methodology  used  to  develop  the  algorithms  for  conyatibility 
with  particular  usar  accuracy  and  environmental  requirements. 
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2.  °T RAP DOWN  COMPUTATION  OPERATIONS 

Figure  1 depicts  tha  computational  elamante  implemented  by  software  algorithms  in 
typical  atrapdown  inertial  navigation  systems.  Input  data  to  the  algorithms  is  provided 
from  a triad  of  strapdown  gyroa  and  acceleromatsra.  Tha  gyros  provide  pracision  maasure- 
menta  of  strapdown  aensor  coordinate  frame  ("body  axaa")  angular  rotation  rate  relative  to 
nonrotating  inertial  space.  The  accelarometars  provida  precision  measurements  of  3-axis 
orthogonal  specific  force  acceleration  along  body  axes. 


BOOY 

ACCELERATIONS 
(FROM  STRAPOOWN 
ACCELEROMETERS) 


BOOY 

RATES 

(FROM  STRAPOOWN 
GYROS) 


FIGURE  1 - STRAPDOWN  ATTITUDE  REFERENCE  OPERATIONS 


The  strapdown  gyro  data  is  processed  on  an  iterative  basis  by  suitable  integration 
algorithms  to  calculate  the  attitude  of  the  body  frame  ralative  to  navigation  coordinates. 
The  rotation  rate  of  the  navigation  frame  is  an  input  to  tha  calculation  from  the  navigation 
section  of  the  overall  computation  software.  Typical  navigation  coordinate  frames  ara 
oriented  with  the  z-axis  vertical  and  tha  x,  y,  axes  horizontal. 

The  attitude  information  calculated  from  the  gyro  and  navigation  frama  rate  data  is  used 
to  transform  tha  accelerometer  specific  force  vector  measurements  in  body  axas  to  their 
equivalent  form  in  navigation  coordinatae.  The  navigation  frame  specific  force 
accelerations  are  then  integrated  in  the  navigation  software  saction  to  calculate  velocity 
and  position.  The  welooity/po6ition  computational  algorithms  are  not  unique  to  the 
strapdown  mechanization  concept,  hence,  are  not  treated  in  this  papar.  Sevaral  texts  treat 
tha  velocity /position  integration  algorithms  in  datall  (1,  2,  3,  4,  12). 

Figure  1 also  shows  an  Eular  Angla  Extraction  function  as  part  of  the  strapdown  attitude 
reference  operations.  This  algorithm  is  used  to  convert  the  calculated  attitude  data  into 
an  output  format  that  is  more  compatible  with  typical  user  requiramants  (e.g.,  roll,  pitch, 
heading  Euler  angles). 


3.  STRAPDOWN  ATTITUDE  INTEGRATION  ALGORITHMS 

The  attituda  information  in  strapdown  inertial  navigation  systems  is  typically 
calculatad  in  the  form  of  a direction  cosine  matrix  oc  as  an  attitude  quaternion.  The 
direction  cosine  matrix  is  a thraa -by-three  matrix  whose  rows  represent  unit  vectors  in 
navigation  axes  projected  along  body  axae-  As  such,  the  alamant  in  the  cow  and  j**1 
column  represents  the  cosine  of  the  angle  between  the  navigation  frame  i-axis  and  body  frame 
j-axia.  The  quaternion  is  a four-vector  whose  elemente  are  defined  analytically  (5,  9)  as 
follows: 

a = ( “v/a)  sin  ( a/2) 

b = ( ay/a)  sin  (a/2)  (1) 

c = ( a*/  a)  sin  ( a/2  ) 

d * cos  ( a/2) 


where 
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a ^ / ay#  a 


Component#  of  an  engla  vector  £. 
Magnitude  of  a. 


Tha  a vector  is  defined  to  have  direction  and  magnitude  such  thet  if  the  nevigetion 
frame  wae  roteted  ebout  a through  en  angle  a,  it  would  be  roteted  into  elignment  with  the 
body  frame.  The  a rotatTon  angla  vector  end  ita  quaternion  equivalent  (a,  b,  c,  d,  from 
equetione  (1)),  or  tha  direction  cosine  matrix,  uniquely  define  tha  ettitude  of  the  body 
axea  relative  to  navigation  exes. 
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3.1  Direction  Cosine  Updetlnq  Algorithms 


3.1.1  Direction  Cosine  Updating  Algorithm  For  Body  Rotetions 

Tha  direction  cosine  matrix  can  be  updeted  for  body  frame  gyro  sensed  motion  in  tha 
strepdown  computer  by  executing  the  following  claasicel  direction  cosine  matrix  chein  rule 
elgorithm  on  e i-epetativa  basis* 


C(m+1)  * C(m)  A(m) 


(2) 


where 

C(a)  " Direction  cosina  matrix  relating  body  to  nevigetion  axaa  at  the  computer 
cycle  time 

A(m)  « Direction  cosine  martix  that  transforms  vectors  from  body  coordinetas  et  the 
(m+1)^  computer  cycle  to  body  coordinetes  et  the  computer  cycle. 

It  is  well  known  (9)  thet: 


A(m)  » I + fi(ix)  + f2(ix)2 


(3) 


where 


■jj-i 


1 - *2/31  + *4/4l  -• 


1 ~ COB  <> 

2 - 1/21  - *2/4l  + *4/6l 


.2 


- $x2  + $„2  + $z2 


(4) 


A 

(ix)  = 


o 

“♦y 


X 


3x3  unity  matrix 


♦ x > 4y' $z 
£ 


Components  of  ^ . 

Angle  vector  with  direction  end  magnitude  such  thet  e rotetion  of  the  body 
frame  about  through  an  angle  equal  to  tha  magnitude  of  £ will  rotate 
tha  body  frame  from  its  orientation  at  computer  cycle  m to  its 
orientation  at  computer  cycle  nH-1.  The  £ vector  is  computed  for 
eech  computer  cycle  m by  processing  the  dete  from  the  strepdown  gyros. 

The  elgorithm  for  computing  £ will  be  described  subsequently. 
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The  "order"  of  the  algorithm  defined  by  equations  (2)  through  (4)  is  determined  by  the 
number  of  terme  carried  in  the  fj,  f2  expansions.  A fifth  order  algorithm,  for  example, 
retains  sufficient  terme  in  fj  and  f2  such  that  A(m)  containe  all  _£  term  products  out  to 
fifth  order.  Hence,  f,  would  be  truncated  after  the  term  and  f2  would  be  truncated  after 
the  i2  term  to  retain  fifth  order  accuracy  in  A(m).  The  order  of  accuracy  required  is 
determined  by  system  accuracy  requirements  under  maximum  rate  input  conditions  when  ^ is  a 
maximum.  The  computation  iteration  rate  is  typically  selected  to  aseure  that  remains 
small  at  maximum  rate  (e.g.,  0.1  radians).  This  assures  that  the  number  of  terms  required 
for  accuracy  in  the  f^,  f2  expansions  will  be  reasonable. 


3.1.2  Direction  Coeine  Updating  Algorithm  For  Navigation  Frame  Rotations 

Equation  (2)  is  used  to  update  the  direction  cosine  matrix  for  gyro  sensed  body  frame 
motion.  In  order  to  update  the  direction  coeines  for  rotation  of  the  navigation  coordinate 
frame,  the  following  claseical  direction  cosine  matrix  chain  rule  algorithm  is  used: 

C(n+1)  * B(n)  C(n)  (5) 


where 


B(n)  “ Direction  cosine  matrix  that  transforms  vectors  from  navigation  axes  at 
computer  cycle  n to  navigation  axes  at  computer  cycle  (n+1). 


The  equation  for  B(n)  parallels  equation  (3): 


B(n)  **  I - (8x)  + 0.5(ex)2 

with 


(6) 


(ex) 


A 


(7) 


where 

ex,ey,ez  * Components  of  j}. 

■ Angle  vector  with  direction  and  magnitude  euch  that  a rotation  of  the 
navigation  frame  ebout  Q_  through  an  angle  equal  to  the  magnitude  of  j) 
will  rotate  the  navigation  frame  from  its  orientation  at  computer  cycle  n 
to  its  orisntation  at  computer  cycle  n+1.  The  £ vector  is  computed  for 
each  computer  cycle  n by  processing  the  navigation  frame  rotation  rate  data 
from  the  navigation  software  section  (12). 


It  ie  important  to  note  thst  the  n cycle  (for  navigation  frame  rotation)  and  m cycle 
(for  body  frame  rotation)  are  generally  different,  n typically  being  executed  et  a lower 
iteration  rate  than  m.  This  is  permissable  becsuse  the  navigation  frame  rotation 
rates  are  considerably  smaller  than  the  body  rates,  hence,  high  execution  ratee  are  not 
needed  to  maintain  jj  smell  to  reduce  the  order  of  the  iteration  algorithm.  The  algorithm 
represented  by  equations  (5)  and  (6)  is  second  order  in  J3.  Generally,  first  order  is  of 
sufficient  accuracy,  and  the  (9.x)2  term  ne-»d  not  be  carried  in  the  actual  software 
implementation . 


3.2  Quaternion  Updating  Algorithms 


3.2.1  Quaternion  Transformation  Properties 

The  updating  algorithms  for  the  attitude  quaternion  can  be  developed  through  an 
investigation  of  its  vector  transformation  properties  (5,  9).  We  first  introduce 
nomenclature  that  is  useful  for  describing  quaternion  algebraic  operations.  Referring  to 
equation  (1),  the  quaternion  with  components  a,  b,  c,  d,  can  be  described  as: 


u 


ai  + bj  + ck  + d 


(8) 
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vhara 
a,b,c 
i,  j.k 


Components  of  the  "vector"  part  of  the  quatarnion. 

Quaternion  vector  operator*  analagou*  to  unit  vactore  along  orthogonal 
coordinate  axes  - 

"Scalar"  part  of  the  quaternion. 


We  also  define  rules  for  quaternion  vector  operator  products  ast 


ii  - -I 

35  ' -1 

kk  - -1 


ij  - k 
jk  - i 
ki  - j 


ji  - -k 
kj  - -i 
ik  - -j 


With  the  above  definitions,  tha  product  w of  two  quaternions  (u  and  v)  becomes: 
w ■ uv  ■ (ai  + bj  + ck  + d)  {ai  + f j + gk  + h) 


■ aeii  + afij  + agik  + ahi 

+ baji  + bfjj  + bgjk  + bhj 

+ caki  + cfkj  + cgkk  + chk 

+ dei  + df  j + dgk  + dh 


■ {ah  + da  + bg  - cf)i 

+ { bh  + df  + ce  - ag ) j 

+ (ch  + dg  + af  - be)k 

+ (dh  - ae  - bf  - eg) 


or  in  "Pour-vector"  matrix  form: 

« I e ' I f d -c  b a 

w * f*  J c d -a  b 


t ' A 

e ' 

r 

L W * 

f‘ 

9' 

a 

h' 

1 L* 

We  also  define  tha  "complex  conjugate"  of  tha  general  quaternion  u in  equation  (8)  ae t 
u*  ■ -ai  - bj  - ck  + d 

We  now  define  a quatarnion  operator  h(mj  for  tha  body  angle  change  £ over  computer  cycle 
m ae : 

(♦*/♦)  sin  U/2) 

h(m)  - <»y/;{  (9) 

{^x/{  J sin  {.)>  /2) 

COB  (*/2) 


where  the  elements  in  tha  above  column  matrix  refer  to  tha  i,  j,  k,  and  scalar  components  of 
h.  Wa  also  defina  a general  vector  v with  ooranponents  vx,  Vy.  vz.  and  a corresponding 
quaternion  v having  the  same  vector  components  with  a zero  scalar  component: 


Using  the  above  definitions  and  the  ganaral  rules  for  quatarnion  algabra,  it  is  readily 
demonstrated  by  substitution  and  trigonometric  manipulation  that: 


h(m)  v h{ra)*  « A1  (jo)  v 


where 


3-6 


A*  (m) 


i ['‘‘S’  i] 


v* 


y. 


A(m) 


At  defined  in  (3). 


Equation  (10),  therefore,  ia  tha  quatarnion  form  of  tha  vector  transformation  equation 
that  transform*  a vactor  from  body  coordinates  at  computer  cycle  (m+1)  to  body  coordinates 
at  computer  cycle  ms 


v*  ■ A(m)  v 


(ID 


where 
v* , v 
v 
v* 


"Thras-vsctor"  form  of  v'  and  v (i.a.,  with  components  vx‘,  vy‘,  vt‘  end  vx, 
The  general  vactor  v in  body  coordinates  at  computer  cycla  (m+1). 

Tha  general  vactor  v in  body  coordinetas  at  con?>uter  cycle  m. 


vy'  vt), 


3.2.2  Quaternion  Updating  Algorithm  For  Body  Motion 

Equation  (10)  with  ite  equation  (11)  dual  can  be  used  to  define  anelegoua  vactor 
transformation  operations  between  body  coordinates  end  navigation  coordinates  at  computer 
cycla  m ass 


q(m)  v’  q(m)* 
C(m)  v' 


(12) 


where 

q(ra) 

v' 

v" 

v<  ,v" 


Quatarnion  relating  body  axes  to  navigation  axas  at  computer  cycla  m. 
Tha  vector  v in  navigation  coordinates. 

Tha  vactor  v in  body  coordinates  at  computer  cycla  m. 

Quatarnion  ("Four  vector")  form  of  v’,  v”. 


The  q qustarniou  has  four  alesoents  (i.e.,  a,  b,  c,  d)  that  ara  updated  for  body  motion 
4 at  each  computer  cycla  m.  The  updating  equation  is  easily  darivad  by  substituting 
equation  (10)  into  (12): 

v"  ■ q(m)  h(m)  v h(m)*  q(m)* 

Using  the  definition  for  the  quatarnion  complax  conjugate,  it  is  raadily  demonstrated 
that: 

h(m)*  q(m)*  - (q(m)  h(m))* 


Thus, 


v"  = q(m)  h(m)  v (h(m)  q(m))< 


But  we  can  also  write  ths  direct  expression: 
v"  » q(m+l)  v q(m+l)* 


Tharafore,  by  direct  comparison  of  tha  lattar  two  equations: 
q ( m+1 ) * q(m)  h(m) 


(13) 
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Equation  ( J 3 ) is  tha  quaternion  equivalent  to  direction  cosine  updating  equation  (2), 
For  computational  purposea,  h{m)  as  dsfinad  in  equations  {9}  is  equivalently: 

f3  *x 

h(ra)  - f3  »y 

fi  Z 

f4 


Bln  (4/2) 


- 0.5(l  - {O.S4)2/3«  + '0.54)4/51  ) 


cos  (♦/2)  * 1 - {0.54)2/21  + (0. 54)4/41 


{0.54)2  . 0.25  (4X2  + 4y2  + 4*2) 


The  "order"  of  the  equation  {13)  and  {14)  updating  algorithm  depends  on  the  order  of  4 
terms  carried  in  h which  depends  on  the  truncation  point  used  in  f3  and  f^.  Ths  rationale 
for  selecting  ths  algorithm  order  and  associated  algorithm  iteration  rate  is  directly 
analagous  to  selection  of  ths  direction  cosins  updating  algorithm  order  {discussed 
previously) . 


3.2.3  Quatsrnion  Updating  Algorithm  For  Navigation  Frame  Rotation 

Equation  (13)  with  (14)  is  used  to  updata  the  quaternion  for  body  frame  motion  sensed  by 
gyros.  In  order  to  update  the  quaternion  for  rotation  of  the  navigation  coordinate  frame, 
an  algorithm  analagous  to  aquation  {5)  {for  the  direction  cosine  matrix)  is  used  with  a 
navigation  frame  rotation  quaternion  r: 


q(n+l) 


r{n)  q{n) 


-0.5  e, 

-0.5  ey 
-°,5  et 
1-0. 5(6/2) 2 


0/2)2 


0.25  Ov2  + 6 2 + 9 2) 


where 

6X,6V,6  * Components  of  8^  as  defined  previously  for  equations 

{ 6 ) and  ( 7 ) . 

The  development  of  equation  (15)  parallels  tha  development  of  (13).  Ths  equation  for 
r{n)  is  a truncated  form  of  the  theoretical  exact  analytical  exprassion  (analagous  to  the 
second  order  truncated  form  of  equation  (14)).  The  82  term  in  equation  (15)  generally  is 
not  required  for  accuracy  {due  to  the  smallness  of  6 in  typical  applications). 

As  for  the  direction  cosine  updating  algorithm  for  navigation  frame  motion,  the 
equivalent  quaternion  updating  algorithm  {equation  (15))  updating  cycle  n need  not  be 
processed  as  fast  as  the  body  rate  cycle  m to  maintain  equivalent  accuracy.  This  is  due  to 
tha  considersbly  smaller  navigation  frame  rotation  rates  compared  to  body  rotation  rates. 


3.2.4  Equivalancies  Between  Direction  Cosins  And  Quaternion  Elements 

The  analytical  equivslancy  between  the  elements  of  the  direction  cosine  matrix  and  the 
attitude  quaternion  can  be  derived  by  oiract  expansion  of  equations  (12).  If  we  define  the 
elements  of  q as: 


equation  (12)  becomes  aftsr  expansion,  factorization  of  v*.  and  neglecting  the  scalar  part 
of  the  v"  and  v’  quaternion  vectora  (i.e.,  carrying  only  the  vsctor  components  v"  and  v1  ): 


v- 


(d2  + a2  - b2  - c2)  2(ab  - cd) 

2(ab  + cd)  (d2  + b2  - c2  -a2) 

2 (ac  - bd)  2 (be  + ad) 


2(ac 


+ bd) 


v' 


Defining  C in  equation  (12)  as: 


Cll 

£12  S13 

c 

m 

C21 

C22  c23 

LC31 

c32  C33 

equation 

(16) 

whsn 

compared  with  (12)  ahowa  that: 

C11 

- 

d2  ♦ 

a2  - b2  - 

c2 

Cl  2 

= 

2(ab 

- cd) 

C13 

■ 

2(ac 

+ bd) 

C21 

■ 

2(ab 

+ cd) 

c22 

= 

d2  ♦ 

b2  - c2  - 

a2 

c23 

= 

2 (be 

- sd) 

C31 

■ 

2(ac 

- bd) 

C32 

■ 

2(bc 

+ ed) 

C33 

■ 

d2  + 

2 2 
e - a'  - 

b2 

(16) 


(17) 


The  converse  of  equation  (17) 
(from  equation  (1))  that  : 

a2  + b2  + c2  + d2  «■  1 


aumewnau 


— — — — 


Using  ths  propsrty 


ths  converse  of  equation  (17)  can  be  shown  (11)  to  be  computeble  from  the  following  sequence 
of  operations: 


p2 

p3 


in 

l + 
l + 
l + 


+ C?2  + C 


2C, 


2c22 

2C33 


" l1 

- T„ 


33 


If  r,  = max  (r,,  r 2,  r3,  Pq),  ther;. 
= 0.5  Pjl/2  signl. 


b = (C2i  i Cl2)/4s 
“ c C13  + c31^/4a 
= (C32  ” c23)/4a 


“previous  J 


c 

a 


If  Pi 


b 

c 

d 

a 


max  (Pj,  P2,  P3,  PQ),  then: 

= 0.5  P21 72  8ign(bprev£oua ) 

= (C32  + C23)/4b 

= (Cl3  - C31)/4b 
= (C2i  + c^2)/4b 


c 

d 

8 

b 


(18) 


If  P3  = max  (P 


lrl'  r2‘  r3*  o 
0.5  P3I/2  sign!' 

= (c21  ~ Cl2)/4c 
= (Cl3  + C31)/4c 
= (C32  + C23)/4c 


3 , P_ ) , then : 


■previous ' 


If  PQ  = max  (P,,  P2,  P3,  PQ),  then: 
d = 0.5“ 


P4  1/2  sign(dprevi0UB) 
“ (C32  - C23)/4d 
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3.3  The  Computation  Of  £ 


3.3.1  Continous  Form 

The  £ "body  attitude  change"  vector  ie  calculated  by  processing  data  from  the  etrapdown 
gyroe.  Under  situation!  where  the  angular  rotation  rate  vector  (eeneed  by  the  gyroe)  lies 
along  a fixed  direction  (i.e.,  ie  nonrotating  in  inertial  space),  the  ♦ vector  is  equal  to 
the  eiiqple  integral  of  the  angular  rats  vector  over  the  time  interval  from  computer  cycle  m 
to  computer  cycle  (m+l)t 


^m+l 

*m 


w dt 


for  casse  when  u ie  nonrotating. 


(19) 


where 

u - Angular  rate  vector  eeneed  by  the  etrapdown  gyros. 


Under  general  motion  conditions  (when  u may  be  rotating),  equation  (19)  hae  the  more 
complex  form  (ae  derived  in  (10)  or  alternatively,  in  Appendix  A): 


a (t ) 


/ (w  + 1/2  a x w + (1  - S.-i-i'ls)  a x(a  x u))dt 

tm  “ a2  (1-sina)  “ 


1 " 


It  can  verified  by  power  series  expansion  that  to  firet  order. 


(1/a2) 


(i  - fJia-a) 

(l-coea) 


1 

~U 


(20) 


Hence,  a(t)  in  equation  (20),  to  third  order  accuracy  in  a can  be  approximated  by: 
t 

a(t)  • / (w  + 1/2  £ x w + _i_  a x(a  x w))dt  (21) 

12 

A second  order  expression  for  a(t)  esn  be  obtained  from  (21)  by  dropping  the  1/12  term. 
An  even  simpler  expression  for  a(tT  is  obtained  by  dropping  the  1/12  term,  and  approximating 
the  a term  in  the  integral  by  the  direct  integral  of  w: 

£(t)  * / u dt 


6£(t)  - 1/2  /fc  £ x w dt  (22) 

fcm 


i = £(t=tro+1)  + Mlt't^) 


An  interesting  characteristic  about  equation  (22)  is  that  its  accuracy  is  in  fact 
compsrabls  to  that  of  third  order  equation  (21).  In  other  words,  the  simplifying  assumption 
of  replacing  a with  (5  in  the  1/2  a x u>  term  is  in  fact  equivalent  to  introducing  an  error  in 
equation  (21)  that  to  third  order,  equals  the  1/12  a_  x (ax  u)  term.  This  property  can  be 
verified  by  simulation  as  well  as  analytical  expansion  under  hypothesized  angular  motion 
conditions . 

Equation  (22)  is  the  equation  that  is  mechanized  in  software  in  most  modern-day 
strapdown  inertial  navigation  systems  to  calcuste  £.  It  can  be  demonstrated  analytically 
and  by  simulation  that  for  representative  vehicle  angular  motion  and  vibration,  equation 
(22)  faithfully  calculates  £ to  accuracy  levels  that  ere  compatible  with  high  performance 
strapdown  inertial  navigation  system  requirements- 

For  situations  where  u is  nonrotating,  the  6£  term  in  (22)  is  zero  and  $ equals  the 
simple  time  integral  or  u over  the  computer  interval  m (i.e.,  the  equation  Tl9) 
approximation).  For  situations  where  w is  rotating  (s  situation  defined  analytically  as 
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“coning"),  the  $J3  term  ie  nonzero  and  must  be  calculated  and  used  as  a correction  to  the  u 
integral  to  proparly  calculata 

It  is  irqportant  to  note  that  the  accuracy  by  which  equation  (22)  approximates  (20)  is 
dependant  on  4 being  small  (e.g.,  less  than  0.1  radian).  In  order  to  protect  the  accuracy 
of  thia  approximation,  the  computer  iteration  rats  rauat  bo  high  anough  that  remains  small 
under  maximum  vehicle  rotation  rata  conditioner 


3.3.2  Recurs iva  Algorithm  Form 

The  implementation  of  equation  (22)  in  a digital  computer  implies  that  a higher  spaed 
integration  summing  operation  be  performed  during  each  body  motion  attitude  update  cycle.  A 
computational  algorithm  for  the  integration  function  can  be  derived  by  first  rewriting 
equation  (22)  in  the  equivalent  incremental  updating  formi 

£(t)  - 1(1)  t f S dt 


tl+1 

11(1+1)  « l£(l)  + 1/2  / !(t)  x u dt 

*1 


1(1+1)  - £(t-tt+1) 


(23) 


1 - £(t-tn+x)  + fifilt-t,**!) 

with  initial  conditions! 

l<t-tm)  - 0 
“ 0 


(24) 


L 


where 

1 * High  speed  computer  cycla  within  the  m body  rata  update  cycle. 


The  intagrals  in  (23)  can  be  replaced  by  analytical  forms  that  are  compatible  with  gyro  ^ • 

input  data  processing  if  u is  raplaced  by  a ganaralized  time  sarias  expansion.  For 
aquations  (23),  it  is  sufficiant  to  approximate  u over  tha  1 to  1+1  time  interval  as  a 
constant  plus  a linear  ramp: 

w * A + B (t  - tt)  (25) 


where  ) • 

A,  B * Constant  vectors- 


Substituting  (25)  in  (23),  and  recognizing  with  tha  equation  (25)  approximation  that: 

A(t1+i  - tjt)  - 1/2  (16(1)  + 18(1-1)) 

1/2  B(ti+1  - tx)2  - 1/2  (aed)  - aed-D) 

where  by  dafinition: 

ABU)  6 Jtt+1  a it 


I • 


* ..  - • - 


• • 


yields  the  dasirad  final  form  for  tha  updating  algorithm: 
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fl£Un)  - *£U)  ♦ 1/2  ) + 1/6  A9U-U)  x as U) 

fcJl+l  *1+1 

ae(Ji)  «■  / « dt  ■ l de 

tl  “ 

fi.U+1)  - j&( i ) + aslO 

i " itt-t.B+l)  * 


(26) 


with  initial  conditions; 

“ lU-O) 

6£(t«tm)  - 6£U-0) 


- 0 
- 0 


Whars 

de  ■ Gyro  output  pulse  vactor.  Each  component  (x.y.z)  raprasants  the  occur ance 

of  a rotation  through  a specified  fixed  angle  increment  about  the  gyro  input 
axis. 

A9  ■ Gyro  output  pulse  vector  count  from  i to  1+1. 


The  computational  algorithm  described  by  equation  (26)  is  used  on  a recursive  basis  to 
calculate  4 once  each  m cycle.  After  ± is  calculated,  the  8 and  _6£  functions  are  reset  for 
the  next  m cycle  ± calculation.  The  iteration  rate  for  1 within  m is  maintained  at  a high 
enough  rate  to  properly  account  for  anticipated  dynamic  u motion  effects.  Section  6. 
describes  analytical  techniques  that  can  be  used  to  aeeess  the  adequacy  of  the  1 iteration 
rate  under  dynamic  angular  rate  conditions. 


3.4  The  Computation  Of  £ 

The  8 vactor  in  equations  (6)  and  (IS)  is  computed  as  a simple  integral  of  navigation 
frame  angular  rate  over  the  n cycle  iteration  period: 

tn+1 

0 - / Q dt  (27) 

*n  “ 

where 

£ “ Navigation  frame  rotation  rate  as  calculated  in  the  navigation  software 

section  (12). 

Standard  recursive  integration  algorithms  can  be  uaad  to  calculate  J}  in  equation  (27) 
(e.g.,  trapezoidal)  over  the  time  interval  from  n to  n+1.  The  update  rate  for  tha 
integration  algorithm  is  selected  to  be  compatible  with  software  accuracy  requirements  in 
the  anticipated  dynamic  maneuver  environment  for  the  user  vehicle. 


3.5  Orthogonality  And  Normalization  Algorithms 

Host  strapdown  attitude  computation  techniquas  periodically  employ  self-consistancy 
correction  algort.hms  as  an  outer-loop  function  for  accuracy  enhancement.  If  the  basic 
attitude  data  is  computed  in  the  form  of  a direction  cosine  matrix,  the  self-conaistancy 
Chech  is  that  the  rows  should  be  orthogonal  to  each  other  and  equal  to  unity  in  magnitude. 
This  condition  is  besed  on  tha  fact  that  the  rows  of  the  direction  cosine  matrix  represent 
unit  vectors  along  orthogonal  navigation  coordinate  frame  axea  as  projected  in  body  axes. 
For  the  quaternion,  the  self-consistancy  Chech  is  thet  the  sum  of  the  squares  of  the 
quaternion  elements  be  unity  (this  can  be  verified  by  operation  on  equation  (1)). 


3.5.1  Direction  Cosine  Orthogonalization  And  Normalization 

The  test  for  orthogonality  between  two  direction  cosine  rows  is  that  the  dot  product  be 
zero.  The  error  condition,  than  is: 
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Eij 

“ Ci  CjT 

where 

Ci 

- ith  row  of  C 

C1 

« )th  row  of  C 

T 

* Transpose 

A calculated  orhogonality  error  Ejj  can  be  corrected  by  rotating  and  Cj  relative  to 

each  other  about  an  axle  perpendicular  to  both  by  the  error  angle  Ej4.  Since  it  is  not 

known  whether  or  Cj  is  in  error,  it  is  assumed  that  each  are  equally  likely  to  be 

generating  the  error,  and  each  is  rotated  by  half  of  E^j  to  correct  the  error.  Hence,  the 

orthogonality  correction  algorithm  isi 


Ci(n+1)  - Cj/n)  - 1/2  EAj  Cj (n) 
Cj(n+1)  - Cj(n)  - 1/2  Eij  C^n) 


(29) 


It  is  easily  verified  using  (29) 
C,(n)  and  C^(n)  is  no  longer  present 
(29).  J 


that  an  orthogonality  error  E^j  originally  present  in 
in  C^(n+1)  and  Oj(n+l)  after  application  of  equation 


The  unity  condition  on  C^  (i.e.,  normality)  can  be  tested  by  comparing  the  magnitude 
squared  of  C^  with  unity* 

Eu  **  1 - Ct  CAT  (30) 


A measured  normality  error  E^  can  be  corrected  with* 
Ci(n+1)  - C^n)  - 1/2  E^  CL  (n) 


(31) 


Equations  (26)  through  (31)  can  be  used  to  measure  and  correct  orthogonality  and 
normalization  errors  in  the  direction  cosine  matrix.  In  combined  matrix  form,  the  overall 
measurement/correction  operation  is  sometimes  written  as> 

Cn+l  " cn+l/2  (I  - Cn  CrT)  cn  (32) 


3. 5. 1.1  Rows  or  Columns  - The  previous  discussion  addreassd  the  problem  of  orthogonal izing 
and  nomalizing  the  rows  of  a direction  cosine  matrix  C.  In  combined  form,  equation  (32) 
shows  that  the  correction  is: 

6C  • 1/2  (I  - CCT)  C (33) 


Equation  (33)  can  be  operated  upon  by  premultiplicaticn  with  C poctmultiplication  by  CT, 
and  combining  terms.  The  result  is: 

6C-  1/2  C (I  - CTC)  (34) 


The  (I  - CTc)  term  in  (34)  is  the  error  matrix  based  on  testing  orthogonality  and 
normality  of  the  columns  of  C.  Thus,  if  the  rows  of  C are  orthonormalized  (i.e.,  6C  is 
nulled),  the  columns  of  C will  also  be  implicitly  orthonormalized.  The  inverse  applies  if 
the  columns  are  directly  orthonormalized  with  (34).  The  question  that  remains  is,  which  is 
preferred?  The  answer  is  related  to  the  real  time  computing  problem  associated  with  the 
calculation  and  correction  of  orthogonalization  and  normalization  errors. 

Ideally,  the  orthogonalization  and  normalization  operations  are  performed  as  an  outer 
loop  function  in  a strapdown  navigation  computer  so  as  not  to  impact  corrputer  throughput 
requirements.  A computational  organization  that  facilities  such  an  approach  divides  the 
orthonormalization  operations  into  submodules  that  are  executed  on  successive  passes  in  the 
outer-loop  software  path.  A logical  division  of  the  orthonormalization  operations  into 
submodules  is  as  defined  by  equations  (26),  (29),  (30),  and  (31). 

This  implies  that  measurement  and  correction  of  orthogonalization  and  normalization 
effects  are  performed  at  different  times  in  the  computing  cycle.  Such  an  approach  is  only 
valid  if  the  orthogonality  and  normalizations  errors  (i.e.,  E^j  and  E^)  remain  reasonably 
stable  as  a function  of  time. 

To  assess  the  time  stability  of  the  orthogonality/normalizstion  error  is  to  investigate 
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the  rate  of  change  of  the  breckatad  terms  in  equetions  (33)  and  (34). 
these  will  be  defined  esi 


For  convenience, 


Er  - (X  - CCT) 

Ec  - (I  - C^c) 

The  time  derivative  of  (35)  is) 


(35) 


er  « - ccT  - ccT 

Ec  * - CTC  - CTC 


(36) 


Expressions  for  C end  CT  cen  be  developed  by  returning  to  equations  (2),  (3),  (5),  and 
(6).  These  equetions  cen  be  reerrengad  to  show  thet  over  a given  time  intervel,  tha  change 
in  C is  given  byt 

AC  - C(A  - 1)  + (B  - I)C 


• • 


• • 


• • 


• • 


which  with  (3)  end  (4)  becomes  to  first  order) 
AC  - C(±x)  - (9x)C 


(37) 


Dividing  by  tha  time  interval  for  the  change  in  C,  recognizing  that  * end  £ ere 
approximately  integrals  of  w end  Q ovar  the  time  interval,  and  letting  the  time  interval  go 
to  zero  in  the  limit,  yields  the  classicel  equetion  for  the  rete  of  chenga  of  Ct 

C * C(oix)  - (Qx)C  (38) 

where 

(ux),  (Ox)  **  Skew  symmetric  matrix  form  of  vectors  u,  ft. 


The  trenspose  of  (38)  is  > 

qT  - - (ux)  CT  + CT  (gx) 


(39) 


Wa  now  substituta  (38)  end  (39)  into  (36).  Aftar  combining  terms  end  applying  equations 
(3b),  ths  finel  result  ist 

ER  w Er  (Qx)  — (Ox)  ER 

(40) 

ic  = Ec  (ux)  - (yx)  Ec 


Equetions  (40)  show  thet  th-x  reta  of  chenge  of  ER  is  proportional  to  ER  and  tha 
navigation  frame  rotation  rate  D,  Wharaas  the  rata  of  chenga  of  E^  is  proportional  to  Ec  end 
tha  body  rotation  rete  y.  Since  a is  generally  much  lergar  than  q,  Ec  is  generally  lergar 
then  Er.  It  can  be  concluded  that  SR  is  mora  stebla  over  time,  hence,  orthonormalizing  tha 
direction  cosine  matrix  rows  (besed  on  tha  ER  measurement)  is  ths  preferred  computational 
approach  if  tha  real  time  computing  problem  is  tekan  into  account. 


• • 


f 


3.5.2  Queternion  Normalizetion 

The  queternion  is  normalized  by  measuring  its  magnitude  squared  compered  to  unity,  end 
adjusting  each  element  proportionally  to  correct  tha  normalization  error.  The  normalization 
error  is  givan  by: 


q q’ 


(41) 


It  is  aesily  verified  using  the  rulas  for  quaternion  algabric  that  Eg  equals  tha  sum  of 
the  squares  of  the  elements  of  q minus  1.  The  corraction  algorithm  is  given  by: 

“ q(n)  - J/2  Eg  q(n)  (42) 


<3(n+l) 


(42) 
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3.6  Direction  Cosine  Versus  The  Quaternion  For  Body  Attitude  Referencing 

The  tradeoff  between  direction  cosine  versus  quaternion  parameters  as  the  primary 
attitude  reference  data  in  strapdown  inertial  systems  has  been  a popular  area  of  debate 
between  strapdown  analysts  over  ths  past  three  decades.  In  its  original  form,  the  tradeoff 
centered  on  the  relative  accuracy  between  ths  two  methods  in  accounting  for  body  angular 
motion.  These  tradeoffs  invariably  svolved  from  ths  differential  equation  form  of  the 
direction  cosine  and  quaternion  updating  equations  and  investigated  the  accuracy  of 
equivalent  algorithms  for  integrating  thess  equations  in  a digital  computer  under  hypoth- 
scized  body  angular  motion.  Invariably,  the  body  motion  investigated  was  coning  motion  at 
various  frequencies  relative  to  ths  computer  update  frequency.  For  these  early  studies,  the 
tradeoffs  generally  demonstrated  that  for  comparable  integration  algorithms,  the  quaternion 
approach  generated  solutions  that  more  accurately  replicsted  the  true  coning  motion  for 
situations  where  ths  coning  frequency  was  within  a decade  of  the  computer  update  frequency. 

As  presented  in  this  paper,  both  the  quaternion  and  direction  cosine  updating  algorithms 
havs  been  bassd  on  processing  of  a body  engle  motion  vector  £ which  accounts  for  sll 
dynamiic  motion  effects  including  coning.  Thsse  updating  algorithms  (equation  (2)  and  (3) 
for  direction  cosines  and  (13)  and  (14)  for  the  quaternion)  represent  exact  solutions  for 
the  attitude  updating  procsse  for  s given  input  angle  vector  Consequently,  the  question 
of  accuracy  for  diffsrent  body  motion  can  no  longsr  be  considered  a viable  tradeoff  area. 

The  principle  tradeoffs  that  remain  between  the  two  approaches  ere  the  computer  memory  and 
throughput  requirements  associated  with  each  in  a strapdown  navigation  system. 

In  ordsr  to  assess  ths  relative  computer  memory  and  throughput  requirements  for  quater- 
nion parameters  versus  dirsetion  cosines,  the  composite  of  all  computer  requirements  for 
each  must  be  assessed.  In  general,  thess  can  be  grouped  into  three  major  computional  arsas: 

1.  Basic  updating  algorithm 

2.  Normalization  and  orthogonalizetion  algorithms 

3.  Algorithms  for  conversion  to  the  dirsetion  cosine  matrix  form  needed  for 
acceleration  transformation  and  Eulsr  angle  extraction 


Basic  Updating  Algorithms  The  basic  updating  algorithm  for  the  quatsrnion  parameters 
is  somewhat  simpler  than  for  direction  cosines  as  expansion  of  equations  (2)  and  (3) 
compared  with  (13)  and  (14)  would  reveal.  This  results  in  both  a throughput  and  memory 
advantage  for  the  quaternion  approach.  Part  of  this  advantage  arises  because  only  four 
quaternion  elements  have  to  be  updeted  corrpared  to  nine  for  direction  cosines.  The 
advantage  is  somewhat  diminished  if  it  is  recognized  that  only  two  rows  of  direction  cosines 
(i.e.,  6 elements)  need  actually  be  updated  since  the  third  row  can  then  be  easily  derived 
from  the  other  two  by  a cross-product  operation  (i.e.,  the  third  row  represents  a unit 
vector  along  the  z-axis  of  the  navigation  frame  as  projected  in  body  axes.  The  first  two 
rows  represent  unit  vectors  along  x and  y navigation  frsme  axes.  The  cross-product  of  unit 
vectors  along  x end  y navigation  axes  equals  the  unit  vector  along  ths  z-navigstion  sxis). 

Normalization  And  Orthogonalization  Algorithms  - The  normalization  and  orthogonalization 
operations  associated  with  direction  cosines  are  given  by  equation  (28)  through  (31).  The 
quaternion  normalization  equation  is  given  by  equations  (41)  and  (42). 

The  normalization  equation  for  the  quaternion  is  generally  simpler  to  implement  than  the 
orthogonalization  and  normalization  equations  for  the  direction  cosines.  If  only  two  rows 
of  the  direction  cosine  matrix  are  updated  (as  described  in  the  previous  paragraph)  the 
direction  cosine  orthogonalization  and  normalization  operations  required  are  half  that 
dictated  by  (2B)  through  (31),  but  are  still  more  than  required  by  (41)  and  (42)  for  the 
quaternion.  Since  the  orthonormalizetion  operations  would  in  general  be  iterated  at  low 
rete,  no  throughput  advantage  results  for  the  quaternion.  Some  memory  savings  may  be 
realized,  however. 

A key  factor  thst  must  be  addressed  relative  to  orthonormalization  tradeoffs  is  whether 
or  not  orthonormalizstion  is  actually  needed  st  sll.  Clearly,  if  the  direction  cosine  or 
quaternion  updating  algorithms  were  implemented  perfectly,  orthonormalization  would  not  be 
required-  It  is  the  author's  contention  that,  in  fact,  the  accuracy  requirements  for 
strapdown  systems  dictate  thet  strapdown  attitude  updating  software  cennot  tolerate  any 
errors  whatsoever  (compared  to  sensor  error  effects).  Therefore,  if  the  attitude  updating 
software  is  designed  for  negligible  drift  and  scale  factor  error  (compared  to  sensor  errors) 
it  will  also  implicity  exhibit  negligible  orthogonalization  and/or  normalization  errors. 

The  ubove  argument  is  valid  if  the  effect  of  orthonormalization  errors  in  strapdown 
attitude  lata  is  no  more  detrimental  to  system  performance  than  other  software  attitude 
error  effects.  This  is  in  fact  the  case,  as  detailed  error  analyses  would  reveal.  Since 
modern-day  general  purpose  computers  used  in  today's  strapdown  inertial  navigation  systems 
have  the  capability  to  implement  attituds  updating  algorithms  essentially  perfectly  within  a 
reasonable  ♦■hroughput  and  memory  requirement,  it  is  the  author's  opinion  that 
orthonormalization  error  correction  should  not  be  needed,  hence,  is  not  a viable  tradeoff 
area  relative  to  the  use  of  quaternion  parameters  versus  direction  cosines. 

Algorithms  For  Conversion  To  The  Direction  Cosine  Matrix  - If  the  basic  calculated 


l • 


> • 


I • 


I • 


! - • 
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attituds  data  is  direction  cosinea  directly,  no  conversion  procsse  ia  required.  For  cases 
where  only  two  rows  of  direction  cosines  ars  updated,  ths  third  row  must  be  generated  by  the 
cross-product  between  the  two  rows  calculated.  For  exaraplei 


C31  “ C12  c23  ~ C13  c22 

c32  " 

c33  “ 


^13  ^21 


- C 


11  ^23 


C11  C22  “ C12  C21 


(43) 


For  quaternion  parameters,  equation  (17)  trust  be  implemented  to  develop  the  direction 
cosine  matrix,  a significantly  more  complex  operation  compared  with  {43)  for  the  two  row 
direction  cosine  approach.  Since  direction  cosine  elements  ars  generally  required  at  high 
rats  {for  acceleration  transformation  and  Euler  angle  output  extraction)  both  a throughput 
and  memory  penalty  ia  accrued  for  the  quaternion  approach.  Ths  penalty  is  compounded  if  ths 
calculated  direction  cosine  outputs  are  required  to  greeter  than  single  precision  accuracy 
(including  computational  round-off  error).  For  noise-frss  acceleration  transformation 
operations  {such  as  may  be  needed  to  effect  an  accurate  system  calibration)  double-precision 
accuracy  is  needed.  The  result  is  that  equation  (17)  for  the  quaternion  versus  (43)  for 
direction  cosines  would  have  to  be  implemented  in  double-precision  imposing  a significant 
penalty  for  the  more  complex  quaternion  conversion  process. 

Tradeoff  Conclusions  - From  ths  above  qualitative  discussion,  it  is  difficult  to  draw 
hard  conclusions  regarding  a preference  for  direction  cosines  vereus  quaternion  parameters 
for  attitude  referencing  in  strapdown  inertial  systems.  Pros  and  cons  exist  for  each  in  ths 
different  tradeoff  arsaa.  Quantitative  comparisons  based  on  actual  software  sizing  and 
computer  loading  studies  have  led  to  eimilar  inconclusive  rseults.  Fortunately,  today's 
computer  technology  ie  such  that  the  slight  advantage  one  attituds  parameter  approach  may 
havs  over  the  other  in  any  particular  application  is  insignificant  compared  with  conposite 
total  atrapdown  inertial  syetem  throughput  and  memory  software  requirements.  Hence, 
ultimate  selection  of  the  attituds  approach  can  be  eafsly  made  based  on  "analyst's  choice". 


4.  STRAPDOWN  ACCELERATION  TRANSFORMATION  ALGORITHMS 

The  acceleration  vector  measurement  from  the  accelerometers  in  a etrapdown  inertial 
system  is  transformed  from  body  to  navigation  axee  through  a mechanization  of  the  claesical 
vector  tranformation  equation: 

aN  » C a (44) 

whsrs 

a “ Specific  force  acceleration  measured  in  body  axes  by  the  strapdown 
accelerometers 

eN  ■ Specific  force  acceleration  with  components  evaluated  along  navigation  axes. 

The  implementation  of  equation  (44)  is  accomplished  on  a repetative  basis  as  a recursive 
algorithm  in  a digital  computer  such  that  its  integral  properties  are  preserved  at  ths 
computer  cycle  times.  In  this  manner,  the  velocity  which  ie  formed  from  the  integral  of 
{44)  will  be  accurate  undsr  dynamic  conditions  in  which  aN  may  havs  erratic  high  frequency 
components.  The  recursive  algorithm  for  {44)  rnuet  account  for  the  effects  of  body  rotation 
{and  secondarily,  rotation  of  ths  navigation  coordinate  frams)  ae  well  as  variations  in  a 
over  the  computer  iteration  period. 


4 . 1 Acceleration  Traneformation  Algorithm  That  Accounts  For  Body  Rotation  Effects 

To  develop  an  algorithm  for  equation  (44)  that  preserves  ite  integral  properties,  we 
begin  with  its  integral  over  a computer  cycle: 

tm+l 

= / C a dt  (45) 

tm  “ 


= Change  in  the  integral  of  equation  (44)  (or  specific  force  velocity  change) 
over  a computer  cycle  m 

The  velocity  vector  in  the  navigation  computer  is  generated  by  summing  the  uN's 
corrected  for  Coriolie  and  gravity  effects. 

The  C matrix  in  (45)  is  a continuous  function  of  time  in  the  interval  from  tm  to  tm+i. 
An  equivalent  form  for  C in  tsrme  of  its  value  at  the  computer  update  time  (m)  is: 


uN 

where 

UN 


C 


C{m)  A(t) 


(46) 


3-16 


where 

C(ra)  » Value  of  C at  tm 

Kit)  x Direction  cosine  matrix  that  traneform  vectore  from  body  axee  at  tlma  t to  the 
body  attitude  at  the  etart  time  for  the  computation  Interval  tm> 


Equation  (46)  with  the  definition  for  Kit)  above  accounte  for  tha  affect  of  gyro  seneed 
body  motion  over  the  computer  Interval.  The  next  section  will  diecuas  the  correction  usad 
to  account  for  tha  small  rotation  of  the  navigation  frame  ovar  the  computer  interval. 


Subatitutlng  (46)  in  (45)  and  expandings 


u« 


C(m) 


/ 


tm+l 

K{t)  a dt 

tm 


We  now  uee  a first  order  approximation  for  Kit)  as  given  by  equation  (3),  with  ± traatad 
as  a function  of  time  in  the  interval  as  defined  to  first  ordar  in  equation  (22): 

£( t)  • £(t)  » / w dt 

*rn 


Thus, 

Mt)  - I + (£.(t)x) 


(47) 


and 


tm+l 

uN  « C(m)  / (I  + (g_( t )x ) ) a dt 
th 


cm+l 


m+l . 


C(u)  (/  a dt  + / (g.(t)  x a)  dt) 


We  now  daflne 


Hence i 


A tro+l 
* / a dt 
t«  “ 


C(m)  (u  + /t|n+1(^(t)  x a)  dt) 


(48) 


» • 


with 


g_(t)  ” / o dt 

till 


• • 


tm+l 

u =»  / a dt 


An  alternative  form  of  (48)  can  also  ba  derived  through  diract  application  of  the 
integration  by  parts  rula  to  tha  Integral  term  in  tha  equation  (48)  u®  expression. s 


C(m)  (u  + 1/2  1 x u + 1/2/  (£(t)  x a + u(t)  x u)  dt) 

tm 


(49) 


with 
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£(t) 

- / J,  dt 

zm 

u(t) 

■ /*  a dt 
tm  “ 

& 

u 

" »(t-tm+i) 

U 


f 


Equations  (48)  and  (49)  ara  algorithmic  forms  of  equation  (44)  that  can  be  uead  to 
calculate  uN  in  the  strapdown  computer  exactly  (within  tha  approximation  of  equation  (47)). 
Thaae  equations  show  that  the  epecific  force  velocity  change  in  navigation  coordinates  is 
approximately  equal  to  tha  integrated  output  from  the  strapdown  acceleromatar  (u)  ovar  tha 
computer  cycle,  times  the  direction  cosine  matrix  which  waa  '-tlld  at  the  previous  computer 
update  time.  Correction  tarms  ara  applied  to  account  for  body  rotation.  In  general,  tha 
correction  term  involves  an  integral  of  tha  intarractive  effects  of  angular  u and  linear  a 
motion  ovar  the  update  cycle.  Tha  integral  terms  have  been  coined  "eculling^  affacte. 

The  equation  (49)  form  of  the  uN  equation  includes  a 1/2  j}  x u term  which  can  be 
evaluated  at  t,.+i  as  the  aimpla  cross-product  of  integrated  gyro  and  accelerometer 
measurements  (i.a.,  without  a dynamic  integral  operation).  Furthermore,  it  ie  easily 
demonstrated  that  for  approximately  conatant  angular  rates  and  accelerations  ovar  the 
conputer  cycle,  the  integral  term  in  (49)  ia  identically  zero.  This  forms  the  basis  for  an 
approximate  form  of  (49)  which  is  valid  under  benign  flight  conditiona  (i.a.,  ueing  equation 
(49)  without  including  the  integral  term).  The  1/2  ji  x u tarn  in  (49)  ie  aometimes  denoted 
as  "rotation  condensation". 


4.1.1  Incremental  Form  of  Transformation  Operations  and  Sculling  Tarms 

In  a severe  dynamic  environment,  equations  (48)  or  (49)  would  be  implemented  explicitly 
with  tha  integral  terms  mechanized  as  a high  spead  digital  algorithmic  operation  within  tha 
tm  to  t^+i  update  cycle.  The  integral  tarms  we  are  dealing  with  are  from  (48)  and  (49): 


Si 


(50) 


S2  ^ 1/2  / tm+^(t)  x a + u(t)  x ju)  dt 

tm 

With  the  aquation  (50)  definitions,  (48)  and  (49)  become: 

uN  » C(m)  (u  + Sj)  (51) 

or 

uN  ■ C(m)  (u  + 1/2  j3  x u + S2)  (52) 

Racureiva  algorithms  for  or  S2  can  be  derived  by  first  rewriting  (50)  in  tha 
equivalent  form: 


_@(t) 
u(t  > 


41 ( 1+1 ) 


j2(  X+l) 

j( A+l) 
u ( i+1 ) 
§1 
$2 


j( x)  + /fc  a dt 
u(i)  + f1,  a dt 

S " 

ji  U)  + j/+1U<t) 

1 1+1 

JT2<  ^ + x/2  Jt 


x a)  dt 

(j(t)  x a + u(t)  x _j»! ) dt 


p(t=ti+i) 

Uft-tj^i) 

Jl(t=,tm+i) 

Jf2(t=tm+1> 


(53) 


• • 


• • 


• • 
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).'•  ' 
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k 


with  initial  conditiona 
^(t-tm)  - 0 

u(t«tm)  * 0 

Il*t“tm>  ° 0 
I2<t-tm)  « o 


(54) 


I 


i 


where 

1 “ High  epaed  computer  cycle  within  m lowar  speed  confutation  cycle. 


The  integrals  in  (53)  can  be  replaced  by  analytical  forma  that  are  compatible  with  gyro 
and  accelerometer  input  data  proceasing  if  u and  a are  replaced  by  a generalised  time  series 
expansion.  For  equations  (53),  it  is  sufficient  to  approximate  w and  a over  the  1 to  1+1 
time  interval  as  conatanta.  Using  this  approximation  in  (53)  yialda  the  final  algorithm 
forms.  For  S^,  the  companion  to  equation  (51),  the  algorithm  ia: 

Il^A+1)  - iiU)  + (p(l)  + 1/2  AflU))  x Av(l) 


£(1+1)  - £(1)  + A0(1) 


where 

tl+l  tji+1 

A0  (Jl ) * / w dt  * l d£ 

fcl  “ tjt 

fcl+l  fcl+l 

Av  (1 ) * / a dt  m l dv 

*1  fcl 


and 

Si  - Y1(t-tnl+1) 

For  equation  (5l)s 

u(l+l)  = u< Jt ) + Av(l) 

A 


(55) 


i 


t • 


r 

! 


with  initial  conditions) 
£(t«tm)  - £(1*0)  » 0 


0 


where 


d0 , dv. 


A0 , Av, 


Gyro  and  accelerometer  output  pulse  vectors.  Each  component  (x,  y,  z) 
represents  tha  occurance  of  a rotation  through  a specified  angle  about  the 
gyro  input  axis  (for  d0  components)  or  an  acceleration  through  a specific 
force  velocity  change  along  the  accelerometer  input  axis  (for  dv 
components) . 

Gyro  and  accelerometer  pulse  vector  counts  from  1 to  1+1. 


I • 


I • 


9 ...  • 
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For  the  alternative  £2  form,  the  companion  to  equation  (52),  the  algorithm  is: 
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I2(l+1)  - X2U)  ♦ 1/2  (1U)  * AW*)  + W*)  * 

£(1+1)  « 1(1)  + A9(t) 
u(l+l)  ” u(l)  + Av(Jl) 
whsra 

fljl+l  nll+l  „ 

A9(l)  - / w dt  « l d0 

fcl  “ fcJl 

,11+1  vtl+l 

Au(l)  - / a dt  « I dv 

tl  ~ tl 

and  (56) 

§2  " 12(^1) 

For  equations  (52): 

1 " i(fWi) 

u " ift-tjo+i) 
with  initial  conditions: 
l(t-t„)  ■ 1(1-0  - 0 

u(t-tra)  - u(l-O)  - 0 
A 

X2<tmtni)  = i2(l“0)  * 0 


Equations  (51)  with  (55),  or  (52)  with  (56)  are  computational  algorithms  that  can  be 
used  to  calculate  tha  navigation  frama  specific  forca  velocity  changes.  Two  iteration  rates 
ars  implied:  a basic  m cycle  rate,  and  a higher  speed  1 cycle  rate  within  each  m cycle. 

The  m cycle  rata  is  selected  to  ba  high  enough  to  protact  the  approximation  of 
neglecting  tha  (ji(t)x)2  term  in  A(t)  (contraat  equation  (47)  with  the  equation  (3)  exact 
form  for  A).  This  design  condition  is  typically  evaluated  under  maximum  expected  linear 
acceleration/angular  rats  envelope  conditions  for  tha  particular  application.  Typically, 
the  m cycle  rete  required  for  accuracy  in  tha  attitude  updating  algorithms  is  alao 
sufficient  for  accuracy  requirements  in  the  m cycle  of  the  acceleration  transformation 
algorithms. 

Tha  Jl  cycle  rata  within  m is  set  high  enough  to  proparly  account  for  anticipated 
coitposite  dynamic  u,  a effects.  Section  6.  describes  analytical  techniques  that  can  be  used 
to  assess  tha  edequacy  of  the  ^ iteration  rata  for  the  sculling  computation  under  dynamic 
input  conditions. 


4.1.3  Acceleration  Transformation  Algorithms  Basad  on  Quaternion  Attitude  Dete 

Equations  (51)  or  (52)  were  based  on  the  use  of  direction  cosine  data  (C)  in  the 
strapdown  computer.  If  the  basic  attitude  data  is  calculated  in  the  form  of  e quaternion, 
the  equivalent  C matrix  for  transformation  can  ba  calculated  using  equations  (17). 
Alternatively,  the  quaternion  data  can  be  applied  directly  in  the  implementation  of  the 
tranformation  operation  through  application  of  equation  (12)  to  equations  (51)  and  (52): 


uN  n q(m)  (u  + S^)  q(m)*  (57) 


or 

u»  = q(m)  (u  + So)  q(ro)* 

(58) 

A 

S2  - 1/2  | * u i Sj 
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where  u and  the  terras  in  the  middle  breckets  are  the  queternion  form  of  the  vector  of  the 
aarae  nonmencleture  definad  as  heving  the  firat  three  terms  (i.e.,  vector  components)  equal 
to  the  vector  elements,  and  the  fourth  sceler  terra  equal  to  zero.  The  S1  and  S2  terras  ere 
celculeted  es  defined  by  equetions  (55)  end  (56). 


4.2 


Acceleration  Transformation  Algorithm  Correction  For  Navigation  Frame  Rotations 


The  acceleration  trenaformation  elgorithms  represented  by  equetion  (51),  (52)  or  (57), 
(58)  with  (55),  (56)  neglecta  the  effect  of  navigetion  frame  rotation.  In  general,  this  is 
e minor  correction  term  that  can  be  easily  accounted  for  et  the  n cycle  updete  rete  (i.e., 
the  computer  cycle  rete  used  to  update  the  ettitude  date  for  ths  effect  of  nevigation  freme 
rotations).  It  can  be  shown  through  a development  similar  to  thet  leading  to  equetion  (52), 
that  the  correction  algorithm  for  locel  nevigation  frame  motion  is  given  to  first  order  by: 


AuN(n)  « - 1/2  e x v(n) 


(59) 


U 


where 

*uN(n) 

v(n) 

e 


Correction  to  tha  velue  of  uN  computed  in  the  m cycle  thet  occurs  at  the 
current  n cycle  time.  (Note?  the  m cycle  ia  within  the  lower  speed  n cycle  time 
frerae) • 

Summation  of  u(m)  over  the  n cycle  updete  period. 

Integral  of  the  nevigation  frame  angular  rotation  reta  over  the  n cycle 
psriod  (as  described  in  Sections  3.1.2  end  3.4) 


5.  EULER  ANGLE  EXTRACTION  ALGORITHMS 

If  the  body  ettitude  relative  to  navigation  axas  is  defined  in  terms  of  three  successive 
Euler  angle  rotetions  4,6,4  about  exes  z,  y,  x respectively  (from  ■'•avigetion  to  body 
exes),  it  cen  be  reedily  demonstrated  (9)  that  the  relationship  between  the  direction  cosine 
elements  end  Euler  engles  is  given  by: 


r 


CU 

“ 

eus6  cos4 

C12 

m 

- cos4  sin4 

+ sin4  sin9  cos^ 

C13 

m 

sin4  s3n4  + 

C0S4  sine 

COSlJ, 

C21 

m 

cos6  sin4> 

c22 

- 

cos4  cos4  + 

sin4  sine 

sirv|j 

c23 

* 

- sin4  cosij. 

+ CO84  sine  sinj, 

C31 

- 

- sin9 

c32 

sin4  cos  6 

C33 

= 

cos4  cos6 

(60) 


for  conditions  where  /8 / * * / 2 the  inverse  of  equations  (60)  can  be  used  to  evaluate  the 
Eular  engles  from  the  direction  cosines: 


tan 


= - tan 


-1  C32 

C33 

-1  C31 


= tan 


/(1-CU2) 

-1  C2i 

Cu 


(61) 


For  situetions  where  /9 / approaches  it/2,  the  4 and  4 equations  in  (61)  become 
indeterminate  because  the  numerator  and  denominator  approach  zero  simultaneously  (see 
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equations  (60))*  Under  these  conditions,  an  alternative  equation  for  4,  4 can  be  developed 
by  first  applying  trigonometric  algebra  to  equations  (61)  to  obtain: 

c23  + C12  “ 

C13  - c22  * 
c23  ~ c12  “ 

Ci3  + C22  - 

Taking  appropriate  reciprocals  of  sine,  cosine  terms  in  (62)  and  applying  ths  inverse 
tangent  functioni 


(ain8  - 1)  ain(4  + 4) 
(ain8  - 1)  cos  (4  + ♦) 
(ain8  + 1)  ain(4  - 4) 
(sin8  + 1)  cos  (4  - 4) 


(62) 


Por  9 near  + */2 


4-4 


-1 

tan 


c23  ~ c12 
C13  + c22 


For  8 near  - s/2 


4 + 4 


-1  c23  + c12 

tan  - 

C13  ~ c22 


(63) 


Equations  (63)  can  be  used  to  obtain  expressions  for  the  sum  or  difference  of  4 and  4 
under  conditions  where  /8/  is  near  s/2.  Explicit  separate  solutions  for  4 and  4 cannot  be 
found  under  the  /8/  ■ s/2  condition  because  4 and  4 both  become  sngle  measures  about 
parallel  axes  (about  vertical),  hence,  measure  the  same  angle  (i.e.,  a degree  of  rotational 
freedom  is  lost,  and  only  two  Euler  angles,  9 « ± s/2  and  4 or  4 define  the  body  to 
navigation  frame  attitude).  Under  /8 / near  s/2  conditions,  8 or  4 can  be  arbitrarily 
selected  to  satisfy  another  condition,  with  the  unspecified  variable  calculated  from  (63). 

As  an  example,  4 might  be  set  to  a constant  at  the  value  it  had  from  equations  (61)  when  the 
/0/  near  s/2  region  was  entered.  This  sslsction  avoids  jumps  in  4 as  the  solution  equation 
is  transitioned  from  the  (61)  to  the  (63)  form. 


* • 


6.  ALGORITHM  PERFORMANCE  ASSESSMENT 

The  division  of  the  ettitude  updating  end  acceleration  transformation  algorithms  into 
high  and  low  speed  loops  for  body  motion  effects  (1  and  m ratee)  provides  for  flexibility  in 
selection  of  the  iteration  rates  to  maintain  overall  algorithm  accuracy  at  system  specified 
performance  levels.  Ths  1 and  m rate  algorithms  have  been  designed  such  that  the  high  rate 
1 loop  consists  of  simple  computations  thet  cen  be  iterated  st  the  high  rate  needed  to 
properly  account  for  high  frequency  vibration  effects.  The  m rate  loop  algorithms,  on  the 
other  are  more  complicated,  based  on  computationally  exact  solutions. 

Iteration  rates  tor  the  m loop  are  selected  to  maintain  auuutacy  under  maximum  maneuver 
induced  motion  conditions.  Ths  m loop  iteration  rate  to  maintain  accuracy  under  maximum 
maneuver  conditions  can  be  easily  evaluated  analytically,  or  by  simulation,  through 
comparision  of  the  actual  algorithm  solution  with  the  Taylor  series  truncated  forms  selectsd 
for  system  mechanization.  Itsretion  rates  for  the  1 loop  are  selected  to  maintain  accuracy 
under  anticipated  vibratory  environmental  conditions. 


6.1  Vibration  Environment  Assessment 


A fundamental  calculation  that  should  be  performed  prior  to  the  analysis  of  * loop 
algorithm  iteration  rate  requirements  is  an  assessment  of  the  dynamic  inputs  that  must  be 
measured  by  the  algorithms.  In  essence,  this  consists  of  an  evaluation  of  the  continuous 
(i.e.,  infinitely  fast  iteration  rate)  form  of  the  algorithms  in  question  under  dynamic 
input  conditions.  Ths  specific  continuous  form  equations  of  interest  are  equations  (22) 
for  and  (50)  for  Sj  or  62* 


6.1.1  Dynamic  Environment  Assessment  (Coning) 

We  repeat  equations  (22)  for  evaluated  at  t = tm+i: 
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£(t)  « / u dt 

"" 

(64) 

SjMt-t,^)  - 1/2  / ni+1£(t)  X u dt 
tm 


end  enalysa  the  solution  for  ^(t^t^+j.)  under  ganerel  cyclic  motion  et  frequency  f in  ares  x 
end  y with  enguler  amplitudes-^*,  ®y  end  relative  phase  angle  ♦ sUch  theti 


w dt  * (©_  sin(2nft),  6 sin(2nft-t*),  o)T 

0 — ” 

(65) 

co  " 2xf  (e*  cos(2*ft),  6y  cos(2xft+*),  0)T 

Substituting  (65)  in  (64),  eitpanding  through  epplicetion  of  epproprieta  trigonometric 
identities,  and  carrying  out  the  indiceted  integrals  enelytically  between  tha  assigned 
limits,  yields  zero  for  the  x,  y components  end  the  following  for  the  z component  of 
l£(t"‘tra+l) 8 


X 6*  ey  (sin*)  f ( <tnH.1 


sin  2xf(t|n+1  - tm)  ^ 


Defining  the  ra  cycle  time  interval  es  Tm, 


. , sin  2xfTm 

* 6*  6y  (sin*)  f ( 1 - “ 


2xfT„ 


the  letter  exression  is  equivalently: 

) (66) 


Hence,  even  though  the  u rate  is  cyclic  in  two  axas  as  defined  by  equation  (65)  in  x end 
y,  the  value  for  is  a constant  proportionel  to  the  sine  of  the  phase  engle  between  the 
x,  y anguler  vibrations.  Undar  conditions  where  ♦ » 0 (defined  es  “rocking"  motion),  is 
zero.  Under  conditions  where  * ■ x/2,  is  maximum.  The  equation  (65)  rata  when  * <=  x/2 
has  been  termed  "coning  motion"  due  to  tne  characteristic  response  of  tha  z exis  undar  this 
motion  which  describes  e cone  in  inertial  space • 

Equation  (66)  cen  be  put  into  a "drift  rate"  form  by  dividing  the  angla  by  the  tima 
interval  Tra  ovar  which  it  wes  evaluated: 


• , . r sin  2xfTm  , 

* * 6x  6y  (sin*)  f ( 1 - __J!1  ) 


(67) 


. Equation  (67)  is  e fundamental  equation  that  can  ba  usad  to  essass  the  magnitude  of 
6pz  that  must  ba  ^ccountad  for  by  tha  6{i  computer  algorithm  under  discrata  fraquancy  input 
conditions.  If  Sf)z  is  small  ralativa  to  system  performance  requiramants,  it  cen  be 
naglectad,  and  the  i loop  algorithm  for  6p  need  not  be  implamentad. 

Equation  (67)  dascribas  how  6j3z  cen  be  celculatad  for  a discreta  input  vibration 
frequency  f.  In  a more  ganeral  case,  tha  input  rate  is  composad  of  e mixture  of  frequencies 
in  x end  y at  diffarent  phese  anglee  * for  eech.  If  the  source  of  tha  gereralized  angular 
vibration  is  random  input  noise  to  tha  strapdown  eystem,  tha  x,  y motion  is  colored  by  the 
transmission  characteristics  of  tha  noise  input  to  the  x,  y angular  response.  A mora 
ganaral  devalopment  of  equation  (67)  thet  accounts  for  the  lattar  ef facts  shows  that  the 
comparable  aquation  for  is  given  by: 

. , , sinufTm 

= J w A*(w)  Ay(w)  sin(*Ay(w)  - *ax(“))  (1  “ -)  pnn(5“)  (69) 

0 % 

where 

Ax(w),  Ay(u)  = Amplitude  of  trensfar  function  relating  system  input  vibration  noisa 
to  angular  attituda  raBponsa  of  sansor  aesambly  about  x,  y axas. 

*Ax(“)* ^Ay(“^  = p^aaa  of  transfer  function  relating  system  input  vibration  noisa  to 

angular  attitude  responsa  of  sansor  assembly  about  x,  y axas. 


Pnn(ju)  **  Power  epectral  density  of  input  vibration  noise, 
u * Fourier  frequency  (rad/eec) 

Notei  Mean  squared  vibration  energy  - J pnn^“^  dw 


Equation  (68)  can  be  used  to  assess  the  extent  of  random  spectrum  dynamic  angular 
environment  to  be  measured  by  the  6fJ  computational  algorithm.  The  valus  calculated  by 

(68)  measures  the  composite  correlated  coning  drift  in  the  sensor  assembly  that  must  be 
calcuated  to  accurately  account  for  the  actual  motion  present.  If  the  magnitude 
calculated  from  (68)  is  small  compared  to  other  systems  error  budget  effects,  the 
mechanisation  of  an  algorithm  to  calculate  is  not  needed  (i-e.,  can  be  approximated  by 
sero) . 

The  extension  of  equations  (67)  and  (68)  to  y,  s or  s,  x axis  angular  vibration  motion 
should  be  obvious. 


6.1.2  , §2  Dynamic  Environment  Assessment  (Sculling) 

We  repeat  equations  (SO)  with  u and  j)  from  (48)  and  (49) i 


l(t)  - 
u(t)  - 

£l 

12  " 

and  analyse  the  Si,  S2  solutions  under  general  cycle  motion  st  frequency  f in  axes  x,  y 
with  angular  amplituHe  9X  about  axis  x and  acceleration  amplitude  Dy  along  axis  y at 
relative  phase  t euch  thati 

Jg  u dt  - ( 6X  sin(2*ft) , 0,  0)T 

u “ (2xf  tfx  cos(2rcft),  0,  0)T  (70) 

a - (0,  Dy  ein(2*ft+*),  0)T 

Substituting  (70)  in  (69),  expanding  through  applicstion  of  appropriate  trigonometric 
identities,  and  carrying  out  the  indicated  integrals  analytically  between  the  assigned 
limits,  yields  zero  for  the  x,  y components  and  the  following  for  the  z component  of 


and  S2< 

s2z  " 

1/2 

sinxfTm 

Tra  9x  Dy  (coe$)  i,1  “ — * - — ) 

(71) 

slz  - 

1/2 

<£  * + s2z 

(72) 

where 

(£  x u) 

z ” 

z - component  of  J!  x u evalulsted  at  t * tm+l* 

Hence,  even 
(70),  the  value 

though  the  w and  a inpute  are  cyclic  in  two  axes 
for  S2k  is  a conotent  proportional  to  the  cosine 

as  defined  in  equations 
of  the  phaee  angle  between 

/!  a dt 


/t  a dt 

V 


/^+1U(t)  x a)dt 


1/2  / m+1(£(t)  x a + u(t)  x a)dt 
tm 


(69) 
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the  x angular  vibration  and  y linear  ecceleretion  vibration*  Undar  conditions  where  #>x/2, 
S2e  is  xaro.  Under  conditions  where  ♦ - 0,  S2x  is  a maximum.  Equation  {70)  motion  When 
♦“  0 has  been  termed  "sculling  motion"  due  to  the  analogy  with  the  characteristic  angular 
movement  end  acceleration  forces  imparted  to  an  oar  used  to  propel  a boat  from  the  stern. 
Note  also  that  S^x  is  equal  to  S2x  plus  the  correction  term  (rotation  compensation)  meesurad 
aa  the  cross-product  of  the  simple  angular  rate  and  linear  acceleration  integrals  taken  over 
the  m computation  cycle.  {See  equations  (48)  and  (49)  for  definitions). 

Equation  (7l)  for  S2x  can  be  put  into  en  "acceleration  bies"  form  by  dividing  the 
velocity  change  correction  S2x  by  the  time  interval  T^  over  which  it  was  evaluated: 

S2x  “ 1/2  «x  D (cost ) (l  - ljn  a>fT|?..)  (73) 

" “Jr  Of  OT* 


Equation  (73), (with  (72)  for  Slx)  is  a fundamental  equation  that  can  be  used  to  assess 
the  magnitude  of  S2_  that  must  be  accounted  for  by  the  Si  or  S2  computer  algorithm  under 
discrete  frequency  Input  conditions.  If  S2x  is  email  relative  to  system  performance  require- 
ments, it  cen  be  neglected,  end  the  1 loop  algorithm  for  calculating  S^  or  S2  need  not  be 
inplemented.  Under  the  latter  conditions,  S^  would  be  set  equal  to  the  cross-product  term 
in  (72)  Which  makes  the  basic  equation  (51)  and  (52)  transformation  algorithms  identical. 

Equation  (73)  deacribes  how  S2x  can  be  calculated  with  a discrete  input  vibration 
frequency  f for  angular  motion  about  x end  linear  notion  along  y.  In  e more  general  case, 
the  input  rates  end  accelerations  are  composed  of  mixtures  of  angular  end  linear  motion 
about  x and  y at  different  frequencies  and  relative  phase  angles.  If  the  source  of  the 
gensralixed  vibration  motion  is  random  input  noise  to  ths  strapdown  system,  the  x,  y angular 
end  linear  motion  is  colored  by  the  transmission  cherecteristics  of  the  noise  input  to  the 
x,  y angular  end  linear  rssponse.  A mors  general  development, of  equation  (73)  thet  accounts 
for  the  latter  effects  show  that  the  comparable  equation  for  S2e  is  given  by: 

s2,  - r <V>  M-l  - •..<*»  - V"> 

sinwTm  <7*) 

~ *By(<*>  ))  (1  - ) Pnn(ju) 


Ax(w),  Ay(u), 
*Ax^“  ) • *Ay{w  J > 
pnn(j“).  “ 


As  defined  previously. 


BX(W),  By(U)), 


x,  y,  amplitude/phese  linear  acceleretion  response  of  the  sensor 
assembly  to  the  input  vibration. 


Equation  (74)  cen  be  used  to  assess  the  extent  of  random  spectrum  dynamic  motion 
environment  to  be  measured  by  the  Sj^  or  S2  coraputetionel  algorithms.  The  S2s  value 
calculated  by  (74)  measures  the  composite  correlated  sculling  acceleretion  bias  in  the 
sensor  assembly  that  roust  be  calculated  to  accurately  account  for  ths  actual  motion  present. 
If  the  S2s  magnitude  calculated  from  (74)  is  small  compared  to  other  system  error  budget 
effects,  the  mechanixation  of  en  algorithm  to  calculate  Sj^  or  S2  in  the  high  rate  1 loop  is 
not  needed  (i.e.,  S2  can  be  approximated  by  zero  in  (52)  or  can  be  set  equal  to  the 
cross-product  term  in  (52)). 

The  extension  of  equations  (73)  and  (74)  for  y,  z or  z,  x axis  vibration  motion  should 
be  obvious. 


Algorithm  Accuracy  Assessment 


The  eccurecy  of  the  computation  algorithm  for  60_  or  S^,  S2  cen  be  assessed  by  compering 
their  solutions  to  the  comparable  continuous  form  solutions  developed  in  Section  6.1  under 
Identical  input  conditions. 


6.2.1  6£,  Coning  Algorithm  Error  Asssssment 

The  computational  algorithm  for  calculating  in  e strapdown  system  is  given  by 
equation  (26).  A measure  of  the  accuracy  of  the  equation  (26)  algorithm  can  be  obtained  by  _•  • 

analytically  calculating  the  solution  generated  from  (26)  under  assumed  cyclic  motion  end 
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comparing  thi ■ result  to  the  equivalent  aolution  obtained  from  the  idealise]  continuoua 
algorithm  deacribed  in  Section  6.1.  For  a diacrete  frequency  vibration  input,  the  equation 
(65)  notion  can  be  uaed  analytically  in  equation  (26)  to  calculate  the  algorithm  aolution 
for  ft]S  at  t * tg+i  (i.e.,  analagoua  to  the  equation  (67)  aolution  for  the  continuoua 
(infinitely  fast)  algorithm.  After  much  algebraic  manipulation,  it  can  be  demonatrated  that 
the  algorithm  aolution  for  6J3  ae  calculated  from  equation  (26)  under  equation  (65)  input 
notion,  haa  sero  x,  y componenta,  with  a s component  rate  given  bys 


6PtALO  “ « «x  flv  ("in  ♦)  (d  + 1/3  U - coa  2*^))  i-  (75) 

2efTi 

ain  2«fT 

JL) 

2lIfTm 


whera 

ftPzALQ  ” Recursive  algorithm  aolution  for  ftpr  rate 

Tji  “ Time  interval  for  high  apeed  1 computer  iteration  cycle 


Equation  (75)  for  the  ft£  diacrete  recuraiva  algorithm  aolution  of  equation  (26)  ie 
directly  analagoua  to  the  equation  (67)  solution  of  the  equation  (22)  continuoua  ftp 
algorithm.  It  ia  easily  verified  that  (75)  reducea  to  (67)  aa  T*  approaches  zero. 

The  error  in  the  ft£  algorithm  ia  meaaured  by  tha  differencs  between  (67)  and  (75);  i.e.: 

e(60s)  ■ * f 6X  6 (ain  #)((1  + 1/3  (1  - coa  2*0“*))  — * — * - l)  (76) 

2«fTt 


where 


e(ftpz)  - Error  rate  in  the  equation  (26)  algorithm. 


Equation  (76)  can  be  uaad  to  aaeeea  the  error  in  the  equation  (26)  algorithm  caused 
by  finite  iteration  rate  (i-e.,  the  effect  of  T*)  under  diacrate  frequency  input  conditions. 

Under  random  vibration  input  conditions,  the  equation  (26)  algorithm  can  be  analyeed  to 
obtain  the  more  ganeral  eolution  for  the  ftp^ALG  rat«' 


^PzALG  “ / “ Ax(u)  Ayfw)  ein($Ay(o)  - ♦ax(w))  (<1 

+ 1/3  (1  - coswTj) 


ein  l>Ti  sin  wT_, 

m)  PbnU“> 


(77) 


wT_ 


The  ftp  algorithm  error  under  random  inputs  is  the  difference  betwean  the  equation  (77) 
discrete  eolution  and  tha  equivalent  continuous  equation  (66)  solution  form.  The  result  is: 


e (6P* ) - J 


Ax(w)  Ay(w)  sin(»Ay(ti>) 


♦ax<“»  (<1 


. , , ein  uTji 

+ 1/3  (1  - cobuT*)  

“Tji 


Equations  (76)  and  (78)  can  be  ueed  to  assess  the  error  in  the  equation  (26)  ftp 
algorithm  caused  by  finite  iteration  rate  under  diacrete  or  random  vibration  input 
conditions.  The  extension  of  equations  (76)  and  (78)  to  y,  z or  z,  x axis  effects  should  be 
obvious. 


l)  Pnn<J“> 


(78) 


6.2.2  S Sculling  Algorithm  Error  Assessment 


The  computational  algorithm  for  calculating  or  Sj  is  given  by  equations  (55)  and 
(56).  A measure  of  the  accuracy  o.:  these  algorithms  can  be  obtained  by  analytically 
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celculeting  the  solution  generated  from  (55)  or  (56)  under  essumed  cyclic  motion  end 
couriering  the  result  to  the  equivelent  solution  obtained  from  the  continuous  algorithm  es 
described  in  Section  6.1.2.  For  e diecrste  frequency  vibration  input,  ths  equation  (70) 
motion  can  be  used  analyticelly  in  equation  (55)  and  (56)  to  celculete  the  algorithm 
solution  for  Sj,  S2  (i.s.,  enelogous  to  the  equation  (72)  end  (73)  aolution  for  the 
continuous  (inrinTtaly  fest)  elgorithm).  After  much  algebreic  manipuletion,  it  can  be 
demonstrated  that  the  algorithm  solution  for  end  S2  es  calculated  from  equations  (55)  and 
(56)  undsr  equation  (70)  input  motion,  hes  zero  z,  y componenta,  with  a z component  rete 
given  byi 


®2tALG 

SltALG 


1/2  6t  Dy  (cos*) 
1/2  (£  x u)B  + S 


sin  2*fT1 
2tALG 


sin  2«fTm 
2*fTm 


(79) 

(80) 


where 

s1eALG»  s2rALG  " Recursive  elgorithm  solutions  for  SlE,  S2b- 

Equations  (79)  end  (80)  for  the  S,,  Sj  discrete  recursive  elgorithm  aolution  is  directly 
enalogous  to  the  equetions  (73)  end  7*2)  aolution  to  the  continuous  Sj.  S2  algorithm.  It  is 
easily  verified  thet  (79)  and  (80)  reduce  to  (73)  end  (72)  as  T*  epproacKas  taro. 

The  error  in  the  S, , S2  elgorithm  is  meesured  by  the  diffarsnce  betwsan  (79),  (80)  and 
(73),  (72)  ; i.e., 

• . • . , , . , »in  2sfTj 

a(S,_)  - e(S2.)  - 1/2  e.  D (cos*)  (1  - *_)  (81) 

2s  f Tjt 

whers 

e(SiE),  e(S2e)  « Error  reta  in  the  equetion  (55)  and  (56)  algorithm  solutions. 


Equation  (81)  cen  be  uaed  to  assess  the  error  in  the  equetion  (55)  end  (56)  algorithms 
ceused  by  finite  iteretion  rete  (i.a.,  the  effect  of  Tj)  under  discrete  frequency  input 
conditions. 

Under  rendora  vibration  input  conditions,  the  equation  (55)  end  (56)  algorithms  can  be 
enelyead  to  obtein  the  more  general  solution  for  Sle,  S2et 


*2s  “ / (Ay(w)  Sx(w)  cos  (♦Ay(io)  - ♦bx(“)) 

- Ax(u)  By(u)  COs(*Ax(w)  - ♦gy('i))))  (— n.  f_  (82) 

- »)  pnn(ju)  du, 

uTm 

slz  “ l/2  <£  * + S2s 

The  S^z,  S2s  algorithm  error  under  vibretion  is  tha  difference  between  the  equetion  (82) 
discrete  solutions  end  the  equivalent  continuous  equation  (74)  with  (72)  forms: 


s(sjs)  = a(S2z)  = / (Ay(w)  Bx(u)  cos(sAy(u)  - 4Bx(u)) 

- Ax(u)  By(u)  cos($Ax(w)  - *8y(w)))  (1  (83) 

sin  uTt 

- -i)  Pnn<5“>  <*« 

“TJt 


Equation  (82)  end  (83)  can  be  used  to  assess  the  error  in  tha  equetion  (55)  end  (56) 
algorithms  caused  by  finite  iteration  rate  under  discrete  or  random  vibration  input 
conditions.  Tha  extension  of  equetion  (83)  to  y,  z or  z,  x exis  affects  should  be  obvious. 
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7.  CONCLUDING  REMARKS 

The  strapdown  computational  algorithms  and  associated  design  considerations  presented  in 
this  paper  are  representative  of  the  algorithms  being  used  in  most  modern-day  strapdown 
inartial  navigation  aystemo.  The  unique  characteristic  of  the  attituda  and  transformation 
algorithms  pressnted  is  the  separation  of  each  into  a complex  low  speed  and  simple  high 

speed  computation  section.  Due  to  the  simplicity  of  the  high  speed  calculations  they  can  be  q ( 

executed  at  the  high  rates  necescary  to  properly  account  for  high  frequecy  but  generally  low 

amplituda  vibratory  effects  without  posing  an  insurmountable  throughput  burdan  on  the 

computar.  The  lower  speed  calculations  which  contain  the  bulk  of  the  computational 

equations  can  than  be  executed  at  a fairly  modest  update  rate  selected  to  properly  account 

for  lower  frequency  but  larger  magnitude  maneuver  induced  motion  effects.  Perhaps  the 

principal  advantage  of  the  algorithm  forms  presented,  is  their  ability  to  be  analyzed  for 

accuracy  using  straight-forward  analytical  techniques-  Thi9  allows  the  algorithms  to  be 

easily  tailored  and  evaluated  for  given  applications  as  a function  of  anticipated  dynamic  f 4 

environments  and  user  accuracy  requirements. 
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APPENDIX  A 

DERIVATION  OF  EQUATION 


A differential  equation  for  the  rate  of  change  of  the  vector  can  be  derived  from  the 
equivalent  quaternion  rate  equation.  The  quaternion  h in  equations  (13)  and  (14)  is  the 
quaternion  equivalent,  to  the  rotation  angle  vector.  A differential  equation  for  h can  be 
derived  from  the  incremental  equivalent  to  (13)  that  describes  how  h changes  over  a short 
time  period  fit  (from  t*  to  tj+^)  within  the  larger  time  interval  from  tm  to  trTT+j^ : 

h(*  +1)  ■=  h(i  ) pd  ) (Al) 


where 


» 
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93  °y 
93  az 
94 


sin  (a/2) 


54  « cos  (a/'l) 


Rotation  angle  vector  associated  with  the  small  rotation 
of  the  body  over  the  short  computer  time  interval  from  X to 
Jt+1  within  the  larger  interval  from  m to  ntf-1 . 


a%,a  ,az,a  = Components  and  magnitude  of  a. 


Equation  (Al)  is  equivalently: 


h('+l)  - h(*> 


At  = ti+1  - tA 


p(*)-l 


The  basic  definition  of  angular  rate  states  that  for  small  At, 


a " u At 

Hence,  for  small  At,  i is  small,  and  therefore,  from  (A2), 

93  * 1/2 

(A5) 

a2 

ga  ” 1 - " 1 - 

2 2 

Using  mixed  vector/scalar  notation,  substitution  of  (A4)  and  (A5)  in  (A2)  yields: 


p - g3  a + g4 

" 1/2  « At  + 1 - 


u2at2 


Substituting  in  (A3)  obtains: 


hU+J  ) - h(X  ) 


- h(X)  ( 1/2  w + 1/2 


In  the  limit  as  At  + 0,  the  latter  reduce  to  the  derivative  form: 

h - 1/2  h u (A6) 

We  now  return  to  (14)  and  express  h as  a function  of  £ in  mixed  vector/scaler  notation: 
h = *3  i.  + £4 
sin  (4/2) 


f4  = cos  (4/2) 

Substituting  in  (A6), 

h = 1/2  f3  4 w + 1/2  f4  <0  (A8 ) 

It  is  readily  demonstrated  by  algebraic  expansion  and  using  the  rules  of  quaternion 
algebra  that  4_  w in  (AS)  is  equivalently: 
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0)  “ * u - • u) 

Differentiation  of  (A7)  shows  that: 


h - h 1 + f3  A + *4 

• cos  #/2  . Bin  */2  . 

f 3 ‘ 1/2  ♦ = ♦ 

♦ *2 

- _L  (1/2  f4  - f3) 

♦ 

f4  * - 1/2  (sini>/2)  ♦ ■ - 1/2  * ♦ f3 

Hence,  with  (AS), 

h = f3  i + —1,  (l/2  f4  - f3)  4 - 1/2  $ 4 f3 

♦ 

" 1/2  f3  (i  x a)  - 1/2  f3  £ • + 1/2  f4  to 

Dividing  by  f3  and  solving  for 

£ = 1/2  . — L.  u + 1/2  x to) 

f3 

- JL  (1/2  _!i-  - 1)  i + 1/2  H - 1/2  i • 2 

♦ f3 


(A9) 


Equation  (A9)  is  now  separated  into  its  vector  and  scalar  components: 

■ f4  $ f4 

1 = 1/2  w + 1/2  (±  x u)  - (1/2  JL  - 1)  1 

f3  ♦ f3  (A10) 

1/2  $ 4>  **  1/2 


The  scalar  equation  is  equivalently: 


♦ 1 


Substituting  in  the  vector  part  of  (.MO)  yields: 


^ = 1/2  — i_  to  + 1/2  (^  x to)  - (1/2  — - 1)  (l  ’ w)  1 

f3  f3 

Using  the  vector  triple  product  rule,  it  is  easily  demonstrated  that: 

(i  ’ i “ 1 x (l  * s)  + “ 

Substituting, 

♦ — 1/2  ....  . to  + 1/2  ^ x u)  - (1/2  — — — — 1)  w + (1  - — — — ) ^ x (j|  x o) 

f3  ~ f3  *2  2f  3 

Combining  terms: 

= 10  + 1/2  | X M + . (1  - ^ X (^  x to) 

*2  2f  3 

Using  the  definition  for  f4  and  f3  from  (A7),  it  can  be  shown  by  trigonometric 
manipulation  that  the  bracketed  coefficient  in  the  latter  expression  is  equivalently: 


*4 

2fl 


(1- 


$ sin  ♦ 
2(l-cos$) 


1 


1 
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Substitution  yields  the  final  expression  for 


, 1 , * sin  * . 

o>  + 1/2  £ x w + [1  - ) $ x (♦  x u) 

~ ~ $2  2(l-cos$) 


(All) 


Equation  (20)  in  the  main  text  is  the  integral  from  of  (All)  over  a computer  cycle  (from  tm 

to  Wl)' 


